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Abstract 

This  paper  presents  a  new  model-based  egomotion  estimation  algorithm  for  an  autonomous  ve¬ 
hicle  navigating  through  rough  terrain-  Due  to  the  uneven  terrain,  the  vehicle  undergoes  bouncing, 
pitch  and  roll  motion.  To  reliably  accomplish  other  tasks  such  as  tracking  and  obstacle  avoidance 
using  visual  inputs,  it  is  essential  to  consider  th^  disturbances.  In  this  paper,  two  vehicle  models 
available  in  the  literature  are  used  for  egomotion  estimation.  The  Half  Vehicle  Model  (HVM)  takes 
into  account  the  bouncing  and  pitch  motion  of  the  vehicle,  and  the  Full  Vehicle  Model  (FVM)  also 
considers  the  roll  motion.  The  dynamics  of  the  vehicle  are  formulated  using  standard  equations 
of  motion.  Assuming  that  depth  information  is  known  for  some  landmarks  in  the  scene  (e.g.,  ob¬ 
tained  from  a  laser  range  finder),  a  feature-based  approach  is  proposed  to  estimate  vehicle  motion 
parameters  such  as  the  vertical  movement  of  the  center  of  mass  and  the  instantaneous  angular 
velocity.  An  Iterated  Extended  Kalman  Filter  (lEKF)  is  used  for  recursive  parameter  estimation. 
Simulation  results  for  both  known  and  unknown  terrain  are  presented. 
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1  Introduction 


There  has  been  growing  interest  among  computer  vision  researchers  in  solving  the  problem  of 
navigating  an  automonous  vehicle  through  uneven  terrain.  The  knowledge  of  the  vehicle’s  pose  and 
motion  relative  to  some  reference  system  is  a  prerequisite  for  the  success  of  other  navigation  tasks 
such  as  object  recognition  and  obstacle  avoidance.  Although  the  Inertial  Navigation  System  (INS) 
on  board  the  vehicle  provides  accurate  motion  information  over  short  periods,  there  are  problems 
over  long  periods  due  to  sensor  drift.  An  independent  estimate  of  the  vehicle’s  motion  can  be 
combined  with  INS  information  to  provide  more  reliable  information. 

In  recent  years,  the  wealth  of  information  contained  in  long  sequences  of  images  has  attracted 
the  attention  of  computer  vision  researchers  [3,  14,  15,  16].  Motion  estimation  based  on  two  or 
three  frames  has  been  shown  to  be  very  sensitive  to  noise  [1,  2,  7,  10],  leading  to  an  increasing 
interest  in  long  sequence  based  methods.  Due  to  lack  of  knowledge  of  the  forces  and  torques 
that  result  in  movements  of  the  camera,  most  model-based  motion  estimation  algorithms  assume  a 
smooth  trajectory  over  time  in  order  to  exploit  temporal  information  [3,  14,  15,  16j.  For  a  vehicle 
traversing  uneven  terrain,  the  camera  undergoes  nonsmooth  motion.  Thus  the  performance  of  these 
algorithms  may  degrade,  depending  on  the  roughness  of  the  terrain,  the  vehicle  speed,  etc. 


In  order  to  describe  the  motion  of  the  vehicle,  both  its  laterzd  dynamics  (or  kinematics)  and  its 
suspension  dynamics  should  be  considered  [13],  in  connection  with  direction  control  and  stabiliza¬ 
tion  behavior,  respectively.  The  work  in  [5]  focuses  on  the  use  of  lateral  vehicle  dynamics  in  3-D 
road  tracking.  In  this  paper,  we  develop  a  motion  estimation  algorithm  mainly  from  the  suspension 
dynamics  point  of  view  and  use  simple  assumptions  about  lateral  kinematics.  Two  vehicle  models, 
which  can  be  found  in  the  literature  on  optimal  design  of  suspension  systems  [4,  8],  are  used  in 
our  work:  A  Half  Vehicle  Model  (HVM),  consisting  of  two  wheels,  which  takes  into  account  vehicle 
bouncing  and  pitch  motion  in  rough  terrain;  and  a  Full  Vehicle  Model  (FVM),  which  considers 


the  correlation  between  the  left  and  right  tr2K:ks,  in  order  to  include  roU  motion  in  addition  to 
boundng  and  pitch  motion.  Both  models  assume  that  each  tire  always  contacts  the  surface  at  a 
point,  tire  stiffness  is  modeled  by  a  linear  spring,  and  the  suspension  system  is  modeled  by  putting 
a  linear  spring  and  a  damper  at  each  corner  of  the  vehicle  body.  Assuming  that  the  vehicle  follows  a 


or 

□ 

□ 


straight  path  with  constant  speed  along  the  longitudinal  axis  (i.e.  there  is  no  steering  control),  the 
motion  of  the  vehicle  can  be  described  using  standard  equations  of  motion.  A  camera  is  assumed 
to  be  rigidly  attached  to  the  vehicle  body  with  known  orientation  relative  to  a  coordinate  system 
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attached  to  the  vehicle.  The  3-D  coordinates  of  some  landmarks  relative  to  a  fixed  reference  system 
are  also  assumed  to  be  available;  these  can  be  obtained,  for  example,  by  a  vehicle-mounted  laser 
range  finder.  Then,  given  the  image  coordinates  of  these  feature  points  at  different  time  instants, 
an  lEKF  [11]  is  used  to  recursively  estimate  the  motion  parameters  of  the  vehicle. 

The  organization  of  this  paper  is  as  follows.  Section  2  gives  detailed  descriptions  of  the  two 
vehicle  models  and  the  physical  laws  governing  the  motion  of  unsprung  and  sprung  masses.  The 
recursive  filter  formulation  of  the  algorithm  is  given  in  Section  3.  Section  4  presents  simulation 
results,  and  conclusions  2u:e  presented  in  Section  5. 


2  Suspension  dynamics 

We  describe  the  vehicle  models  and  their  behaviors  under  various  surface  inputs.  In  both  models, 
an  inertial  coordinate  system  /  is  fixed  on  the  ground.  A  vehicle  coordinate  system  V  moves  with 
the  vehicle  with  its  origin  at  the  vehicle’s  center  of  mass  and  its  axes  coinciding  with  the  principal 
axes  of  the  vehicle’s  body.  When  there  is  no  confusion,  the  time  dependence  of  all  quantities  will 
be  suppressed  in  the  following  derivations. 


2.1  The  half  vehicle  model  (HVM) 

A  two-wheel  vehicle  model  is  shown  in  Fig.  1.  It  represents  the  bouncing  and  pitch  motions  of  the 
vehicle  body,  which  result  from  different  surface  inputs  to  the  front  and  rear  wheels.  M^/  and  Mun- 
represent  the  unsprung  masses  of  the  front  and  rear  wheels  and  their  axles.  Each  tire  is  modeled 
by  a  linear  spring  with  stiffness  coefficient  Kx-  Kf,Cf,KT  and  Ct  are  the  characteristics  of  the 
linear  springs  and  dampers  that  model  the  suspension  system. 

Assuming  that  the  pitch  motion  is  small,  so  that 

sin  ^  ^ 


holds,  and  that  the  variations  of  the  springs  and  dampers  are  along  the  vertical  direction,  the 
motion  of  the  vehicle  body  can  be  described  as  follows  [4]: 


M-uijXi  =  Kfd2  4-  Cfd2  —  Kxd\ 

MyrrX2  —  Krd^  -^  Crd^  —  J^T'da 

MbX  =  -{Kfd2  -t-  C}d2)  -  {Krdi  ^^4) 

19  =  -{Kjd2  +  Cfd2)WA  +  {Krd4  -H  Crdi)WB 
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Figure  1:  The  half  vehicle  model  [4]. 


where  X  is  the  displacement  of  the  center  of  mass  of  the  vehicle,  0  is  the  pitch  angle,  and  I 
represents  the  moment  of  inertia  with  respect  to  the  pitch  axis,  which  is  normal  to  the  half-vehicle 
plane.  The  displacements  of  the  connection  points  {d,,  1  =  1, . .  .,4}  are  approximated  by 

di  —  X\  —  iQi 

d2  =  X  Wa0  -  Xi 

(2) 

ds  =  X2  —  *02 

d4  -  X-Wb0~X2 

where  xoii®02  the  surface  excitation  inputs  to  the  front  and  rear  wheels,  respectively. 

2.2  The  full  vehicle  model  (FVM) 

A  four-wheel  vehicle  model  is  shown  in  Fig.  2.  For  a  vehicle  following  a  straight  path .  in  addition 
to  bouncing  and  pitching  behavior,  rolling  due  to  the  different  inputs  to  left  and  right  wheels  is 
considered  in  this  model.  As  in  the  HVM,  the  tires  au-e  modeled  by  linear  springs,  and  linear  springs 
and  dampers  are  used  to  model  the  suspension  system. 

Wheel  Motion 

For  simplicity,  we  refer  the  heights  of  the  four  wheels  and  the  four  corners  of  the  vehicle  body 
above  the  flat  ground  when  the  vehicle  is  at  rest  (i.e.  when  gravity  is  the  only  externad  force  acting 
on  the  vehicle)  as  reference  heights.  Let  {xi,X3, xs,®?}  be  the  unsprung  mass  displacements, 
{x2, X4, xe, xg}  be  the  displacements  of  the  four  corners,  and  {loi,  t  =  1, . . . , 4}  be  roaid  excitation 
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Figure  2:  The  full  vehicle  model  [8]. 


inputs.  All  these  quantities  are  measured  with  respect  to  their  own  reference  heights,  as  shown  in 
Fig.  2.  Assuming  that  the  linear  springs  and  dampers  in  the  model  are  restricted  to  move  only 
vertically,  the  displacements  between  the  different  connection  poin*^s  are 


di 

=  Xi 

- 

dz 

~  X2- 

Xi 

dz 

=  X3 

1 

H 

0 

dA 

-  X4- 

®3 

ds 

=  Xs 

-  ®03> 

de 

=  X6- 

*5 

dj 

=  X7 

“  X04> 

ds 

=  Xs- 

xr 

and  the  motion  of  the  unsprung  masses  is  described  by  Newton’s  law  as 

MxufXi  =  Kfd2  +  Cfd2  —  Kjd\ 

=  Kfd^  +  Cjdi  —  Kjdz 
MrurX^  =  Kfd^  +  Crd^  —  Kxds 
MrurXj  —  Kfdg  +  Crdg  —  Kxdj 


(3) 


(4) 


Bouncing 

In  Fig.  3,  let  and  be  the  axes  of  the  coordinate  systems  I  and  V  respectively. 

Here  again,  the  suspension  forces  {Fi,  i  =  1, . . . ,  4}  which  act  on  the  four  corners  are  always  in  the 
t-direction,  i.e. 

Fi  =  —(^Kfd2  +  Cfd2)i  =  -Fit 

F2  =  -(Kfd4-hCfd4)i  =  F2i 

Fz  =  -{Krde  +  Crde)i  =  F^i 

F4  =  -{Krds  +  Crds)i  =  ^4? 
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Vehicle  bouncing  is  thus  restricted  to  the  i-direction  and  is  described  by 

MgX^j^Fi  (6) 

t=i 

where  Mg  is  the  mass  of  the  vehicle’s  body  and  X  is  the  displacement  of  its  center  of  mass. 

Pitch  and  Roll  Motions 

As  shown  in  Fig.  3,  the  four  suspension  forces  which  act  on  the  four  corners  at  the  same  time  result 
in  change  of  the  vehicle’s  angular  momentum.  In  order  to  describe  the  pitch  and  roll  motion  of  the 
vehicle’s  body,  we  need  to  find  its  orientation  with  respect  to  the  inertial  coordinate  system — in 
other  words,  we  need  to  find  the  rotation  matrix  which  aligns  V  with  I.  There  are  many  ways  to 
represent  this  rotation  matrix  [9].  Since  the  instantaneous  angular  velocity  changes  with  time,  we 
use  a  quaternion  representation.  In  the  following,  a  vector  P  is  denoted  by  Pi  if  its  components 
are  represented  in  /,  and  by  Pv  if  they  are  represented  in  V. 


Figure  3:  The  inertial  coordinate  system  /  and  the  vehicle  coordinate  system  V . 


Let  yiy  =  {wx,  Wy,  Wz)^  be  the  instantaneous  angular  velocity  of  the  vehicle’s  body  with  respect 
to  the  vehicle  coordinate  system  V.  By  choosing  the  axes  of  V  to  coincide  with  the  principal  axes 
of  the  vehicle’s  body,  Euler’s  equations  of  motion  [12]  can  be  applied  to  find  the  instantaneous 
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angular  velocity: 


Mx  ^xx^x  +  '^y'^zi-fzz  ~~  ^yy} 

My  —  lyyWy  +  WzXDxi^IxX  ~  I zz)  (^) 

^  Mz  y  Izz^z  +  WxWy{Iyy  “  Ixx)  ^ 

where  {Ixx^Jyyilzz}  are  the  moments  of  inertia  of  the  vehicle  with  respect  to  the  coordinate  axes 
of  V  (these  are  constsmts,  because  V  moves  with  the  vehicle’s  body)  and  (Mx,  My,  Mz)^  is  the  net 
torque  applied  by  the  suspension  forces,  with  its  components  expressed  in  V. 

The  instantaneous  angular  velocity  and  the  rotation  matrix  are  related  through  the  quaternions 
as  follows:  Let  the  quaternions  q  be  represented  by 


Then  the  dependence  of  £  on  w  is  [16] 


where 


=  (9i 

>925  93^94)^ 

£  = 

a[tnj]  5 

0 

—Viz 

Wy 

-wx 

VIZ 

0 

-Wx 

—Wy 

—wy 

wx 

0 

-wz 

wx 

wy 

wz 

0 

^[m]  =  2 


and  (wx,wyi  are  the  components  of  the  instantaneous  angular  velocity  represented  in  I.  The 
rotation  matrix  can  then  be  obtained  as  follows: 


R[^  = 


9i  -  92  -  ?!  +  94  2(5192  +  9394) 


2(91 93  -  9294) 


2(91 92  -  9394)  -9i  +92  -93  +  94  2(5293  +  9194) 

2(9193  +  9294)  2(5253-5154)  -91  -92  +  93  +94 

T"!  r2  r3 
r4  rs  re 
r-r  rs  rg  ; 


When  the  rotation  matrix  (10)  is  available,  the  relative  orientation  of  I  and  V  is  known.  The 
coordinates  of  a  vector  P  in  the  two  systems  are  now  related  through  the  rotation  matrix,  i.e. 


=  Pi 
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3  Recursive  filter  formulation 

Unlike  batch  estimators,  which  process  all  the  data,  recursive  filters  update  their  estimates  of  the 
parameters  as  new  information  becomes  available.  Among  recursive  estimators,  EKF  and  lEKF 
are  both  suitable  for  estimating  the  parameters  of  a  nonlinear  system.  The  measurement  equations 
in  HVM  and  FVM  are  similar;  we  will  therefore  describe  only  the  corresponding  plant  equations. 

3.1  The  plant  equations 
HVM 


X  =  {Xi,Xi,X2,X2,X,X,O,0,vy 

be  the  state  vector,  where  v  is  the  vehicle  speed  along  the  longitudinal  axis,  expressed  in  system  I. 
The  plant  equation  is  obtained  from  (1)  and  (2)  as 

i  =  Ax  +  Bxo  (12) 

where 

'  01000  0  0  0 
-^K,■^KT)  ~C,  K,  C, 

Myfj 

00010  0  0  0 


-(K,  +  Kr) 


-(Cf  +  Cr) 
Mg 

0 


i-CfWj^  +  CrWg) 

7  I 

0  0 


-{KfW^-KrWg) 

Mg 

0 


-(Cf  W^-C.Wb) 
Mb 
1 

-(.CfWl  +  CrfVj,) 
I 


_  (  0  ife  0  0  0  0  0  0  0 


Vo  0  0  0  0  0  ( 

where  the  forward  speed  is  assumed  to  be  constant. 


0  0  j  ^ 


Xoi  X02 


FVM 


Again,  let 


X  =  (xi ,  Xi ,  X3, 13,  Xs,  Xs,  X7,  X7,  X,  X,  W/ ,  £^,  v)^ 
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be  the  state  vector,  where  v  is  the  vehicle’s  forward  speed,  represented  in  I.  Note  that  for  conve¬ 
nience,  the  angular  velocity  is  expressed  in  /.  Assuming  that  the  vehicle  moves  along  a  straight 
path  on  the  horizontal  plane  with  constant  speed,  the  plant  equation  is  obtained  from  (4),  (6),  (7) 
and  (8)  as 

i.  —  /(s.,  Xq)  (13) 


where 


/(£,Xo) = 


R{q) 


Xi 

^(AVd2-hCyd2-A'rda) 

j^{Kfd4  4-  Cfd4  -  Krdz) 
is 

jtMrds  +  Crde-KTds) 
i? 

-^^(Ards  +  Crdg  —  Krdj) 
i 

+  ^2  -t-  As  +  Aj) 

—  UyUz{Izz  —  lyy)) 

■j^{My  —  Uz'^xi^xx  ~  ^zz)) 

■^{Mz  —  WlWy(/yy  —  Ixx)) 


/ 

0 

—ijJZ 

Ufy 

-U3X 

\ 

(jJZ 

0 

-UX 

—UJY 

—U7Y 

Ux 

0 

-^z 

v 

ux 

U>Y 

uz 

0 

/ 

V 


0 


/ 


(14) 


Assuming  that  the  movements  of  the  linear  springs  and  shock  absorbers  in  the  j  and  ^-directions 
are  negligible  compared  to  their  movements  in  the  i-direction,  d2,d4,de  and  d^  are  approximated 

by 

dz  =  X  —  xi  +  t^Tz  -f- 

d4  =  AT  -  *3  -  r2Tz  -I-  t^Wa 

de  =  X  —  xs  +  T2Ts  —  t-^Wb 

dg  =  X  —  X7  —  rsTj  -  r^Ws 


(15) 
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where  rjjTa  are  the  elements  of  the  rotation  matrix  (10),  and  Ts,Wa  and  Wb  are  the  dimensions 
of  the  vehicle’s  body,  as  shown  in  Fig.  3. 


In  the  above  formulation,  uj  is  used  to  represent  the  angular  velocity  in  the  state  vector  in 
order  to  preserve  the  dynamics  of  the  quaternions  (8).  To  include  Euler’s  equations  of  motion 
in  the  plant  equation,  the  dynamics  of  the  angular  velocity  u  should  be  expressed  in  the  inertial 
coordinate  system  I.  The  representation  of  u  in  the  V  coordinate  system  can  be  found  from  (11): 


wy  =  RF[q]wj 


(16) 


Because  of  the  characteristics  of  the  angular  velocity  [12],  it  follows  that 


(17) 


where  wj  is  the  rate  of  change  of  the  angular  velocity  with  components  represented  in  system  /. 
Finally,  the  relationship  between  and  i  is  derived  in  Appendix  A. 

As  shown  above,  to  describe  roll  motion  in  addition  to  the  bouncing  and  pitch  motions,  the 
linear  plant  equation  of  HVM  is  replaced  by  a  nonlinear  equation  in  FVM. 

3.2  The  measurement  equations 

Consider  the  camera  coordinate  system  and  the  imaging  model  shown  in  Fig.  4.  Let  Pj  =  (X'^  ,  , 

coordinates  of  the  feature  point,  let  Ov{t)  =  {X{t)  +  h,0,vt)^  be  the  coordinates 
of  the  vehicle’s  center  of  mass  at  time  i  (both  in  /),  and  let  h  be  the  reference  height  of  the  center 
of  mass.  Assuming  that  the  camera  is  fixed  with  respect  to  the  vehicle  system  V,  i.e.  its  projection 
center  Oc{t)  in  V  is  {do,  0,1)^,  then  F’^(t)  =  {x^{t),y^{t),z^{t))^ ,  the  coordinates  of  the  feature 
point  in  the  camera  coordinate  system  C,  are 


(18) 


where  Ro  aligns  the  camera  coordinate  system  C  with  the  vehicle  coordinate  system  V.  For 
simplicity,  Rq  is  set  equal  to  the  identity  matrix  in  our  discussion. 

The  image  plane  coordinates  of  the  feature  point  at  time  t,  can  now  be  obtained  by  applying 
the  perspective  projection  formula.  The  resulting  measurement  equations  for  the  recursive  filter 

9 


' 

fxO 

^  X{t)  +  h  ^ 

* 

^  do  ^ 

=  Rq  < 

R^ig)  ’ 

yi 

— 

0 

►  — 

0 

^  2^(0  j 

1  J 

J 

.  '  y 

Figure  4:  The  camera  coordinate  system  C  and  the  imaging  model 


''  '  iq(Pi  ~  ov(ti))  - 1  • 


_  R'^(pj-ovm 

'*■'  ' Rupj-ovm-i  ' 

where  /  is  the  focal  length  and  N  is  the  number  of  available  feature  points.  {RJ,  i  =  1, 2, 3}  is  the 
row  of  the  rotation  matrix  and  {nXjiU),  takes  into  account  measurement  noise  such 

as  quantization  noise.  Note  that  if  the  HVM  is  employed,  the  rotation  matrix 

^  cos(^)  0  —  sin(0)  '' 

0  10 
^  sin(^)  0  cos(^)  y 

should  be  used  for  R  in  (19),  while  the  quaternion  representation  of  the  rotation  matrix  in  (10) 
should  be  used  for  the  FVM. 

After  the  plant  and  measurement  equations  have  been  formulated,  EKF  or  lEKF  can  be  applied 
to  estimate  the  vehicle  dynamics. 


4  Experimental  results 

Natural  terrain,  in  general,  can  be  decomposed  into  low-frequency  and  high-frequency  components; 
the  low-frequency  component  maintains  the  road’s  shape,  and  the  high-frequency  component  takes 
into  account  its  micro-roughness.  In  the  following  simulation,  we  generated  the  micro- roughness 
from  a  first  order  Markov  process  and  superimposed  it  on  designed  shapes  in  order  to  obtain  the 
surface  excitation  inputs  to  the  front  wheels.  Since  we  assume  that  the  vehicle  moves  along  a 
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straight  path  with  constant  speed,  the  inputs  to  the  rear  wheels  are  the  same  as  the  inputs  to  the 
front  wheels  except  for  a  time  delay,  i.e. 


*02(0  =  XQi{t-Llv)  (20) 

for  HVM,  and 


XQsit) 

=  XQi(t-L/v) 

(21) 

104(0 

=  X02{t  -  L/v) 

(22) 

for  FVM,  where  L  is  the  length  of  the  vehicle.  Fig.  5  shows  the  surface  inputs  used  in  our  simulation. 
Note  that  in  the  case  of  the  FVM,  during  time  interval  [0, 4.0]  the  excitation  inputs  to  the  left  and 
right  wheels  are  the  same,  so  that  no  roll  motion  should  result.  The  front  left  wheel  then  encounters 
a  bump  during  the  time  interval  [4.0, 7.2],  which  mzdces  the  vehicle  start  rolling.  After  the  bump, 
the  surfaces  are  again  the  same  for  both  the  left  and  right  wheels. 

X  itf’ 


Figure  5:  The  excitation  inputs  to  the  front  wheels 


The  values  of  the  vehicle  parameters  are  listed  in  Table  1  [8].  For  the  HVM,  the  plant  equation 
in  (12)  is  first  discretized.  The  ground  truths  of  the  states  are  then  obtained,  with  the  initial 
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Table  1:  Vehicle  parameters 


Mb 

1710  kg 

Kt 

200.0  kN/m 

L 

2.69  m 

Mxuf 

57.5  kg 

Ks 

18.0  kN/m 

Wa 

1.353  m 

75  kg 

Cf 

1.0  kN/m/sec 

Ts 

0.595  m 

Ixx 

1233.05  kg-m^ 

Kr 

10.0  kN/m 

do 

0.6  m 

lyy 

1031.25  kg-m^ 

Cr 

1.0  kN/m/sec 

1 

1.35  m 

hz 

201.79  kg-m^ 

h 

2.0  m 

conditions  arbitrarily  chosen  as 

(0.1  -  0.3  -  0.05  0.4  0.03  0.06  0.01  0.04  1.345)^ 

The  resulting  bouncing  and  pitch  motions  of  the  vehicle’s  body  are  shown  in  Fig.  6.  Note  that  the 
left  wheel  input  shown  in  Fig.  5  is  used  as  the  terrain. 


X 10'  Vcnkal  Bovemeaiof  teocMcrofnus  nio'  Pncbaoole 


Figure  6:  The  time  history  and  bias  of  the  estimates  of  the  motion  parameters  of  the  HVM:  (a) 
vertical  movement  of  the  center  of  mass,  (b)  pitch  angle,  (c)  vertical  movement  error,  and  (d)  pitch 
angle  error. 
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For  the  FVM,  since  the  plant  equation  (13)  is  nonlinear,  a  numerical  integration  technique  is 
employed  with  the  initial  conditions 

(0.03  1.2  0.1  -1.5  -0.05  1.0  -0.02  -0.9  0.01  0.5  -0.1  0.2  0.3  0.1  0.2  0.8  0.55678  2.69)^ 

The  resulting  trajectories,  corresponding  to  the  states  directly  related  to  the  bouncing  and  orien¬ 
tation  of  the  vehicle’s  body,  are  shown  in  Fig.  7.  The  nonzero  instantaneous  angular  velocity  for 
the  FVM  introduces  roll  motion,  in  addition  to  pitch  motion,  from  the  beginning. 


xio*  Vertical  MovemeM  of  (he  coMeraf  mitt  of  (be  i^btde  Bomcae  veiociiy  of  the  center  of  nais  of  its  vebidc 


Figure  7:  The  time  history  of  the  motion  parameters  of  the  FVM:  (a)  vertical  movement  of  the 
center  of  mass,  (b)  bouncing  velocity,  (c)  instantaneous  angular  velocity,  and  (d)  quaternions. 

Eight  feature  points  were  used  in  both  models.  Instead  of  randomly  generating  these  feature 
points,  we  used  feature  points  from  the  Rocket  ALV  sequence,  in  which  ground  truth  is  available 
[6,  15].  Note  that  in  this  above  sequence,  the  camera  moves  along  a  straight  path  toward  the  left. 
In  our  experiment,  we  shifted  each  point  upward  and  to  the  right  by  10.0  so  that  the  corresponding 
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ima^e  point  would  be  close  to  the  center  of  the  image.  To  avoid  the  depth  becoming  negative  after 
a  few  frames,  each  point  was  moved  away  from  the  origin  by  20.0.  The  resulting  3-D  coordinates 
of  the  feature  points  in  the  inertial  coordinate  system  are  listed  in  Table  2.  The  size  of  the  image 
is  assumed  to  be  2.0  x  2.0  with  resolution  200  x  200.  This  is  equivalent  to  adding  0.01  uniform 
noise  to  the  image  plane  coordinates  of  each  feature  point.  The  trajectories  of  the  feature  points 
are  then  obtained  from  (19);  the  sampling  rate  is  10  frames/sec  for  both  models. 

Table  2:  The  feature  point  coordinates  in  the  inertial  coordinate  system  /. 


Feature  points 

3-D  coordinates 

1 

2.317 

-2.245 

41.460 

2 

0.48 

-4.894 

48.559 

3 

1.594 

-0.526 

43.899 

4 

1.526 

9.516 

44.770 

5 

1.632 

1.155 

44.620 

6 

-0.493 

-0.500 

53.794 

7 

3.937 

5.892 

34.35« 

8 

1.739 

6.001 

44.246 

In  the  following,  we  assume  that  the  3-D  coordinates  of  the  feature  points  in  the  inertial  coor¬ 
dinate  system  /  are  available.  Simulations  with  known  and  unknown  terrain  will  now  be  presented. 

Known  Terrain 

Assuming  that  the  excitation  input  to  each  wheel  is  known,  fifty  Monte  Carlo  trials  were  done  for 
both  models.  The  initial  guess  for  the  recursive  filter  was  set  to  be 

(1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0)^ 

for  the  HVM,  and 

(1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0  0.0  0.0  0.0  1.0  1.0)^ 

for  the  FVM.  Note  that  for  the  FVM,  the  initial  guess  for  the  quaternions  should  be  of  length  1 
[16];  the  initial  guesses  were  purposely  chosen  to  be  different  from  the  initial  conditions  in  order 
to  test  the  convergence  of  the  lEKF.  The  estimates  for  the  two  models  are  shown  in  Fig.  6  and 
Fig.  8.  As  shown  in  these  figures,  the  lEKF  converges  to  the  true  values  in  about  40  frames  for 
both  models.  Since  the  plant  equation  of  the  FVM  is  nonlinear,  a  numerical  integration  technique 
was  employed  in  the  prediction  stage  of  the  lEKF  for  the  FVM. 


'0  10  20  30  40  50  40  70  so  90  100  "0  10  20  30  40  50  40  70  80  90  100 

(c>  W) 

Figure  8:  Bias  of  the  estimates  of  the  motion  parameters  of  the  FVM:  (a)  vertical  movement  error, 
(b)  boimcing  velocity  error,  (c)  angular  velocity  error,  and  (d)  quaternion  error. 

Unknown  Terrain 


We  first  consider  the  HVM.  By  modeling  the  inputs  as  first  order  Markov  processes  and  augmenting 
the  state  vector  to  include  the  two  inputs,  the  plant  equation  (12)  becomes 


A  B 
0  a/2 


(23) 


where  /2  is  a  2  x  2  identity  matrix  and  a  is  a  constant.  For  simplicity,  and  W2{t)  are  assumed 
to  be  independent  white  Gaussian  noise,  although  correlation  exists  between  the  inputs  to  the  front 
and  rear  wheels.  Using  the  same  measurement  equations  as  before,  the  lEKF  was  applied,  with 
the  same  initial  guess  as  in  the  case  of  known  terrain. 


The  estimated  results  are  shown  in  Fig.  9.  To  illustrate  the  effects  of  measurement  noise  on  the 
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lEKF,  the  estimates  obtained  when  there  is  no  measurement  noise  are  shown  in  (a)  and  (b).  The 
estimates  with  0.001  uniform  noise,  corresponding  to  an  image  resolution  of  2000  x  2000,  are  shown 
in  (c)  and  (d).  The  state  corresponding  to  bouncing  is  more  sensitive  to  measurement  noise  than 
the  pitch  angle  state,  and  the  errors  in  the  estimates  are  larger  than  those  for  known  terrain.  We 
have  also  observed  that  when  we  treat  the  excitation  inputs  as  additional  states,  the  lEKF  cannot 
track  the  states  corresponding  to  the  wheel  movements  and  inputs.  In  the  case  of  known  terrain, 
this  problem  did  not  appear.  The  observability  of  the  augmented  HVM  (23)  should  be  investigated. 


Figure  9:  The  effects  of  measurement  noise  on  the  lEKF  when  the  excitation  inputs  are  unknown: 
(a)  vertical  movement  error  without  noise,  (b)  pitch  angle  error  without  noise,  (c)  vertical  movement 
error  with  noise,  and  (d)  pitch  augle  error  with  noise. 


We  see  from  the  above  simulation  that  more  information  seems  to  be  required  in  order  to  obtain 
reliable  estimates  of  vehicle  motion  when  the  surface  inputs  are  not  available.  Such  additional 


information  could  be  obtained  if  a  Digital  Terrain  Model  (DTM)  or  an  active  sensor  such  as  LADAR 
were  available.  The  rough  estimates  of  the  surface  could  then  be  combined  with  the  visual  inputs 
to  provide  more  measurements  for  the  lEKF.  Currently  we  are  working  on  this  idea  and  plan  to 
test  it  on  the  FVM. 

5  Conclusion 

A  new  algorithm  has  been  presented  for  estimating  the  egomotion  of  an  autonomous  vehicle  which 
navigates  through  uneven  terrain.  In  this  paper  the  vehicle  is  assumed  to  move  along  a  straight  path, 
but  steering  dynamics  (or  kinematics)  could  also  be  included,  to  take  lateral  motion  into  account. 
Our  work  can  be  directly  applied  to  stabilize  the  images  acquired  during  vehicle  navigation.  The 
pitch  and  roll  angles  can  be  estimated  from  the  instamtaneous  angular  velocity  or  from  the  rotation 
matrix  for  the  FVM.  Using  knowledge  of  the  vertical  movement  of  the  vehicle  and  of  the  pitch  and 
roll  angles,  image  stabilization  can  then  be  achieved. 
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Appendix  A 


To  apply  Euler’s  equations  of  motion  as  stated  in  Section  3,  we  need  to  express  ( My,  MzY 
terms  of  the  state  vector 

Consider  Fig.  3  and  assume  that  {Fi,i  =  always  act  along  the  i-direction,  i.e.  Fi  in 

/  =  (T;x,0,0)^;  then 

Fi\riV  =  {Fr,Fiy,Fizf  (24) 

=  (n  rj  r^fFix  (25) 


where  (ri  r-2  r^)  is  the  first  row  of  the  rotation  matrix  i2{£]  in  (10). 

Let  the  position  vectors  of  the  four  corners  of  the  vehicle’s  body  with  respect  to  coordinate 
system  V  be  {fi,  i  =  1, . . ., 4},  i.e. 

ri  =  iO,Ts,WAf  ri  =  (0,-T„W^x)^ 

r-3  =  (0,T.,-W^b)^  u  =  {0,-Tz,-WBf 

The  net  torque  applied  by  the  four  forces  can  then  be  obtained  as  follows: 


4 

ri  xFi  mV 

t=sl 


(26) 


rjPPs  —  r2WA  -rsTz  —  r2W>i  rsT,  +  r2WB  -raT,  +  r2WB 
riWA  r^WA  -riWB  -tiWb 

-’’iTj  riTa  —riTs  viTa 


'  Fix  ^ 
F2X 
Fzx 

<  ■^4X  / 


Substitution  of  (5)  and  (15)  into  (26)  yields  the  desired  relationship  between  (Mx,  My,  Mz)^  and 
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